#include "imgui_my_adjust.h"
#include "imgui.h"
#include <iostream>

DynamicKalmanAdjuster::DynamicKalmanAdjuster(std::string _name, CoordPredictor &_coordPredictor)
    : Control(_name), coordPredictor(_coordPredictor)
{
    // Read initial matrix and offset data from XML
    cv::FileStorage fs("./../Configure/Settings.xml", cv::FileStorage::READ);

    cv::Mat processNoise, measurementNoise;
    fs["kalman_Q"] >> processNoise;
    fs["kalman_R"] >> measurementNoise;
    cv::FileNode node1 = fs["cam2gyro_offset"];
    cv::FileNode node2 = fs["gun2cam_offset"];
    for (int i = 0; i < 3; ++i)
    {
        cam2gyro_offset[i] = (float)node1[i];
        gun2cam_offset[i] = (float)node2[i];
    }

    for (int i = 0; i < 6; ++i)
        for (int j = 0; j < 6; ++j)
            Q[i][j] = processNoise.at<float>(i, j);

    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            R[i][j] = measurementNoise.at<float>(i, j);
}

void DynamicKalmanAdjuster::Show()
{
    if (!ImGui::Begin(this->name.c_str()))
    {
        ImGui::End();
        return;
    }

    // Q matrix
    ImGui::Text("Q matrix (6x6):");
    ImGui::BeginTable("Qmat", 6, ImGuiTableFlags_Borders);
    for (int i = 0; i < 6; ++i)
    {
        ImGui::TableNextRow();
        for (int j = 0; j < 6; ++j)
        {
            ImGui::TableSetColumnIndex(j);
            char label[32];
            snprintf(label, sizeof(label), "Q[%d][%d]", i, j);
            ImGui::SetNextItemWidth(60);
            if (ImGui::DragFloat(label, &Q[i][j], 0.01f, -1000.0f, 1000.0f, "%.3f", ImGuiSliderFlags_AlwaysClamp))
            {
                this->ApplyQMatrix();
            }
        }
    }
    ImGui::EndTable();
    if (ImGui::Button("Update Q"))
    {
        this->ApplyQMatrix();
        // No logic, UI only
    }

    ImGui::Separator();

    // R matrix
    ImGui::Text("R matrix (3x3):");
    ImGui::BeginTable("Rmat", 3, ImGuiTableFlags_Borders);
    for (int i = 0; i < 3; ++i)
    {
        ImGui::TableNextRow();
        for (int j = 0; j < 3; ++j)
        {
            ImGui::TableSetColumnIndex(j);
            char label[32];
            snprintf(label, sizeof(label), "R[%d][%d]", i, j);
            ImGui::SetNextItemWidth(60);
            if (ImGui::DragFloat(label, &R[i][j], 0.01f, -1000.0f, 1000.0f, "%.3f", ImGuiSliderFlags_AlwaysClamp))
            {
                this->ApplyRMatrix();
            }
        }
    }
    ImGui::EndTable();
    if (ImGui::Button("Update R"))
    {
        this->ApplyRMatrix();
        // No logic, UI only
    }

    ImGui::Separator();

    // cam2gyro_offset
    ImGui::Text("cam2gyro_offset:");
    ImGui::PushID("cam2gyro_offset");
    if (ImGui::DragFloat3("##cam2gyro_offset", cam2gyro_offset, 0.01f, -1000.0f, 1000.0f, "%.4f",
                          ImGuiSliderFlags_AlwaysClamp))
    {
        this->ApplyCam2GyroOffset();
    }
    ImGui::SameLine();
    if (ImGui::Button("Update cam2gyro"))
    {
        this->ApplyCam2GyroOffset();
        // No logic, UI only
    }
    ImGui::PopID();

    ImGui::Separator();

    // gun2cam_offset
    ImGui::Text("gun2cam_offset:");
    ImGui::PushID("gun2cam_offset");
    if (ImGui::DragFloat3("##gun2cam_offset", gun2cam_offset, 0.01f, -1000.0f, 1000.0f, "%.4f",
                          ImGuiSliderFlags_AlwaysClamp))
    {
        this->ApplyGun2CamOffset();
    }
    ImGui::SameLine();
    if (ImGui::Button("Update gun2cam"))
    {
        this->ApplyGun2CamOffset();
        // No logic, UI only
    }
    ImGui::PopID();

    ImGui::Separator();

    ImGui::Text("Extra Prediction Time:");
    ImGui::PushID("extra_prediction_time");
    if (ImGui::DragFloat("##extra_prediction_time", &extra_prediction_time, 0.001f, -5.0f, 20.0f, "%.5f",
                         ImGuiSliderFlags_AlwaysClamp))
    {
        this->ApplyExtraPredictionTime();
    }
    ImGui::SameLine();
    if (ImGui::Button("Update extra_prediction_time"))
    {
        this->ApplyExtraPredictionTime();
    }
    ImGui::PopID();

    ImGui::End();
}

void DynamicKalmanAdjuster::ApplyQMatrix()
{
    cv::Mat Qmat(6, 6, CV_32F);
    for (int i = 0; i < 6; ++i)
        for (int j = 0; j < 6; ++j)
            Qmat.at<float>(i, j) = Q[i][j];
    coordPredictor.updateProcessNoise(Qmat);
}

void DynamicKalmanAdjuster::ApplyRMatrix()
{
    cv::Mat Rmat(3, 3, CV_32F);
    for (int i = 0; i < 3; ++i)
        for (int j = 0; j < 3; ++j)
            Rmat.at<float>(i, j) = R[i][j];
    coordPredictor.updateMeasurementNoise(Rmat);
}

void DynamicKalmanAdjuster::ApplyCam2GyroOffset()
{
    cv::Point3f offset(cam2gyro_offset[0], cam2gyro_offset[1], cam2gyro_offset[2]);
    std::lock_guard<std::mutex> lock(coordPredictor.cam2gyro_offset_Mutex);
    coordPredictor.updateCam2GyroOffset(offset);
}

void DynamicKalmanAdjuster::ApplyGun2CamOffset()
{
    cv::Point3f offset(gun2cam_offset[0], gun2cam_offset[1], gun2cam_offset[2]);
    std::lock_guard<std::mutex> lock(coordPredictor.gun2cam_offset_Mutex);
    coordPredictor.updateGun2CamOffset(offset);
}

void DynamicKalmanAdjuster::ApplyExtraPredictionTime()
{
    coordPredictor.updateExtraPredictionTime(extra_prediction_time);
}